# coding: utf-8
## @package faboMPU9250
#  This is a library for the FaBo 9AXIS I2C Brick.
#
#  http://fabo.io/202.html
#
#  Released under APACHE LICENSE, VERSION 2.0
#
#  http://www.apache.org/licenses/
#
#  FaBo <info@fabo.io>

import time
import sys
import numpy as np
import pandas as pd

from MPU9250 import MPU9250
from FallDetect import FallDectect 
from draw import draw

mpu9250 = MPU9250()
f = FallDectect()
s_t = time.time() # 开始时间
data_arr = [] # 数据缓冲区

try:
    print("begin ...")
    while True:
        now  = round(time.time() - s_t, 3) # 实时时间
        accel = mpu9250.readAccel()        # accelerate 水平加速度 
        gyro = mpu9250.readGyro()          # gypo 陀螺仪（角加速度）
        # print(f"T = {now}", end='\t')
        # print(" ax = " , ( accel['x'] ), end='\t')
        # print(" ay = " , ( accel['y'] ), end='\t')
        # print(" az = " , ( accel['z'] ), end= '\t|')
        # print(" gx = " , ( gyro['x'] ), end='\t')
        # print(" gy = " , ( gyro['y'] ), end='\t')
        # print(" gz = " , ( gyro['z'] ))
        data_arr.append((now, 
            accel['x'], accel['y'],accel['z'],
            gyro['x'], gyro['y'], gyro['z'])
        ) # 记录数据
        f.detect_fall(data_arr[-1])        # 摔倒检测
        time.sleep(0.01)                   # 采样间隔时间

except KeyboardInterrupt:
    # 保存
    print("Saving File ...")
    data = pd.DataFrame(data_arr,columns=['t','ax','ay', 'az', 'gx', 'gy', 'gz'], dtype=float)
    t_t = round(time.time() - s_t, 3)
    path = f"h3/task2/data/mpu_data_{t_t}".replace('.','_')+'csv'
    data.to_csv(path,index=None)
    # 绘图
    print("drawing ...")
    draw(path)  
    print("end ...")
    sys.exit()
